from ros2webserver.baseserver.capability import Capability
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovarianceStamped
import json
from tf_transformations import euler_from_quaternion, quaternion_from_euler
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy


class NavGetOdom(Capability):
    def __init__(self, protocol):
        super().__init__(protocol)  # Modern Python style for calling super()
        self.protocol.register_operation("odomData", self.send_odom)
        
        # Subscriber for odometry data
        self.odom_msg = None
        self._odom_sub = self.protocol.node_handle.create_subscription(
            Odometry,
            "odom",
            self.odom_callback,
            10
        )

        # Subscriber for pose data
        # 创建一个QoS配置，与发布者对齐
        self.custom_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=10  # 您可以根据实际需要调整深度
        )
        self.pose_msg = None
        self._pose_sub = self.protocol.node_handle.create_subscription(
            PoseWithCovarianceStamped,
            "amcl_pose",
            self.pose_callback,
            self.custom_qos
        )

        self._odom_msg = {
            "op": "odomData",
            "code": 1,
            "msg": "failed",
            "data": {
                "linearSpeed": 0.0,
                "angleSpeed": 0.0,
                "xPose": 0.0,
                "yPose": 0.0,
                "angle": 0.0,
            }
        }


    def odom_callback(self, msg):
        self.odom_msg = msg

    def pose_callback(self, msg):
        self.pose_msg = msg

    def send_odom(self, message):
        data = {
            "linearSpeed": 0.0,
            "angleSpeed": 0.0,
            "xPose": 0.0,
            "yPose": 0.0,
            "angle": 0.0,
            'naviStatus':'unkonw'
        }
        if self.odom_msg:
            data['linearSpeed'] = self.odom_msg.twist.twist.linear.x
            data['angleSpeed'] = self.odom_msg.twist.twist.angular.z
        
        if self.pose_msg:
            pose = self.pose_msg.pose.pose
            orientation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
            # Assuming nav_handle.get_current_pose returns orientation in euler angles
            _, _, yaw = euler_from_quaternion(orientation)
            data.update({
                "xPose": pose.position.x,
                "yPose": pose.position.y,
                "angle": yaw
            })

        data['naviStatus'] = self.protocol.nav_handle.getActivateState()
        if data['naviStatus']=='wait' or data['naviStatus']=='unkonw':
            data['naviStatus'] = self.protocol.nav_handle.nav_status
        # if data['naviStatus'] == 'reached':
            # data['naviStatus'] = 'wait'
        self._odom_msg['msg'] = "ok"
        self._odom_msg['data'] = data
        self.protocol.send(self._odom_msg)

    def finish(self):
        self.protocol.node_handle.destroy_subscription(self._odom_sub)
        self.protocol.node_handle.destroy_subscription(self._pose_sub)
